{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "e1e9b90a",
   "metadata": {},
   "outputs": [
    {
     "ename": "FileNotFoundError",
     "evalue": "[Errno 2] No such file or directory: ''",
     "output_type": "error",
     "traceback": [
      "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[1;31mFileNotFoundError\u001b[0m                         Traceback (most recent call last)",
      "\u001b[1;32m~\\AppData\\Local\\Temp/ipykernel_22744/202769394.py\u001b[0m in \u001b[0;36m<module>\u001b[1;34m\u001b[0m\n\u001b[0;32m      8\u001b[0m \u001b[1;31m#控制轮廓顶点\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m      9\u001b[0m \u001b[1;33m\u001b[0m\u001b[0m\n\u001b[1;32m---> 10\u001b[1;33m \u001b[1;32mwith\u001b[0m \u001b[0mopen\u001b[0m\u001b[1;33m(\u001b[0m\u001b[1;34m''\u001b[0m\u001b[1;33m,\u001b[0m \u001b[1;34m'r'\u001b[0m\u001b[1;33m)\u001b[0m \u001b[1;32mas\u001b[0m \u001b[0mf1\u001b[0m\u001b[1;33m:\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m     11\u001b[0m     \u001b[0mlist1\u001b[0m \u001b[1;33m=\u001b[0m \u001b[0mf1\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mreadlines\u001b[0m\u001b[1;33m(\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m     12\u001b[0m \u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;31mFileNotFoundError\u001b[0m: [Errno 2] No such file or directory: ''"
     ]
    }
   ],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "import numpy as nmp\n",
    "import math\n",
    "#degree. 3次样条\n",
    "knotType = 1   #knot的计算方式 0：平均取值  1：根据长度取值\n",
    "CPointX = []\n",
    "CPointY = []\n",
    "#控制轮廓顶点\n",
    "\n",
    "with open('', 'r') as f1:\n",
    "    list1 = f1.readlines()\n",
    "\n",
    "for i in range(len(list1)):\n",
    "    str = list1[i]\n",
    "    str1 = str.split(\",\")\n",
    "    CPointX.append(float(str1[0]))\n",
    "    CPointY.append(float(str1[1]))\n",
    "\n",
    "n = len(CPointX)\n",
    "knot = list(nmp.arange(n+3+1))\n",
    "for i in range(0,4):\n",
    "    knot[i] = 0.0\n",
    "    knot[n+i] = 1.0\n",
    "\n",
    "if knotType:\n",
    "    L = list(nmp.arange(n-1))\n",
    "    S = 0\n",
    "    for i in range(n-1):\n",
    "        L[i] = nmp.sqrt(pow(CPointX[i+1]- CPointX[i],2) + pow(CPointY[i+1]- CPointY[i],2))\n",
    "        S = S + L[i]\n",
    "\n",
    "    tmp = L[0]\n",
    "    for i in range(4,n):\n",
    "        tmp = tmp + L[i-3]\n",
    "        knot[i] = tmp / S\n",
    "else:\n",
    "    tmp = 1 / (n-3)\n",
    "    tmpS = tmp\n",
    "    for i in range(4,n):\n",
    "        knot[i] = tmpS\n",
    "        tmpS = tmpS + tmp\n",
    "print(knot)\n",
    "#节点向量\n",
    "#knot = [0,0,0,0,0.25,0.4,0.8,1,1,1,1]\n",
    "\n",
    "def de_boor_cal(u,knot,X, Y):\n",
    "    #判断u在哪个区间\n",
    "    m = len(knot) #节点个数 且 m = n + k +1\n",
    "    n = len(X)\n",
    "    i = 3\n",
    "    for i in range(3,m-3):\n",
    "        if u>=knot[i] and u<knot[i+1]:\n",
    "            break\n",
    "    #递归计算\n",
    "    if knot[i+3] - knot[i] == 0:\n",
    "        a41 = 0\n",
    "    else:\n",
    "        a41 = (u - knot[i]) / (knot[i+3] - knot[i])\n",
    "    if knot[i-1+3] - knot[i-1] == 0:\n",
    "        a31 = 0\n",
    "    else:\n",
    "        a31 = (u - knot[i-1]) / (knot[i-1+3] - knot[i-1])\n",
    "    if knot[i-2+3] - knot[i-2] == 0:\n",
    "        a21 = 0\n",
    "    else:\n",
    "        a21 = (u - knot[i-2]) / (knot[i-2+3] - knot[i-2])\n",
    "\n",
    "    XP40 = X[i]\n",
    "    YP40 = Y[i]\n",
    "    XP30 = X[i-1]\n",
    "    YP30 = Y[i - 1]\n",
    "    XP20 = X[i-2]\n",
    "    YP20 = Y[i - 2]\n",
    "    XP10 = X[i-3]\n",
    "    YP10 = Y[i - 3]\n",
    "\n",
    "    XP41 = (1 - a41) * XP30 + a41 * XP40\n",
    "    YP41 = (1 - a41) * YP30 + a41 * YP40\n",
    "\n",
    "    XP31 = (1 - a31) * XP20 + a31 * XP30\n",
    "    YP31 = (1 - a31) * YP20 + a31 * YP30\n",
    "\n",
    "    XP21 = (1 - a21) * XP10 + a21 * XP20\n",
    "    YP21 = (1 - a21) * YP10 + a21 * YP20\n",
    "    if knot[i+3-1] - knot[i] == 0:\n",
    "        a42 = 0\n",
    "    else:\n",
    "        a42 = (u - knot[i]) / (knot[i+3-1] - knot[i])\n",
    "    if knot[i-1+3-1] - knot[i-1] == 0:\n",
    "        a32 = 0\n",
    "    else:\n",
    "        a32 = (u - knot[i-1]) / (knot[i-1+3-1] - knot[i-1])\n",
    "\n",
    "\n",
    "    XP42 = (1 - a42) * XP31 + a42 * XP41\n",
    "    YP42 = (1 - a42) * YP31 + a42 * YP41\n",
    "\n",
    "    XP32 = (1 - a32) * XP21 + a32 * XP31\n",
    "    YP32 = (1 - a32) * YP21 + a32 * YP31\n",
    "\n",
    "    if knot[i+3-2] - knot[i] == 0:\n",
    "        a43 = 0\n",
    "    else:\n",
    "        a43 = (u - knot[i]) / (knot[i+3-2] - knot[i])\n",
    "\n",
    "    P43 = [(1 - a43) * XP32 + a43 * XP42,(1 - a43) * YP32 + a43 * YP42]\n",
    "    return P43\n",
    "\n",
    "\n",
    "plt.plot(CPointX,CPointY, '-ob', label=\"Waypoints\")\n",
    "plt.grid(True)\n",
    "plt.legend()\n",
    "plt.axis(\"equal\")\n",
    "\n",
    "Bspline = [[],[]]\n",
    "for u in nmp.arange(0.0,1.0,0.0005):\n",
    "    P = de_boor_cal(u,knot,CPointX,CPointY)\n",
    "    Bspline[0].append(P[0])\n",
    "    Bspline[1].append(P[1])\n",
    "\n",
    "Bspline[0].append(CPointX[-1])\n",
    "Bspline[1].append(CPointY[-1])\n",
    "plt.plot(Bspline[0],Bspline[1], 'r')\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d71cc9a3",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.9.7"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
